/*
 * patio1.c
 *
 *  Created on: May 26, 2021
 *      Author: kirk_
 */

#include "patio1.h"
#include "openmv.h"
#include "movement.h"
#include "direction.h"
#include "distance.h"
#include "light.h"
#include "display.h"

#define Patio1_Task1 1
#define Patio1_Task2 1
#define Patio1_Task3 1

const float AnglePerPixel = 0.088;
//const uint8_t AngleUnitMove = 500;
//const uint8_t AngleUnitInterval = 1000;
#define Pixel2Angle(d)  ((float)(AnglePerPixel * (d)))

#if Patio1_Task1
#define Patio1_Task1_BreakNumber   4U
#define Patio1_Task1_UnitMoveOff   32U
#define Patio1_Task1_UnitMoveInc   32U
#define Patio1_Task1_UnitMoveBound 2U
//#define Patio1_Task1_BrakeRatio    ((float)1.1)
//#define Patio1_Task1_UnitMove      40U
//#define Patio1_Task1_UnitMoveLong  180U
#define Patio1_Task1_UnitTurn     ((float)4.8)
#define Patio1_Task1_CheckDistanceTimes 10U
#define Patio1_Task1_DistanceKeep      400U
#define Patio1_Task1_LineBoundInitial    12U
uint8_t Patio1_Task1_LineBound = Patio1_Task1_LineBoundInitial;
#define Patio1_Task1_Interval     250U
#define Patio1_Task1_TotalMoveBound  44000U
//#define Patio1_Task1_TotalMoveBound  1000U
#define Patio1_Task1_DistanceBound   360U
uint32_t Patio1_Task1_TotalMove = 0;
#endif

#if Patio1_Task2
#define Patio1_Task2_PreferredDistance  220U
#define Patio1_Task2_UnitMoveX          200U
#define Patio1_Task2_MoveTimes           26U
#define Patio1_Task2_MoveInterval       400U
#endif

#if Patio1_Task3
#define Patio1_Task3_PreferredDistance  220U
#define Patio1_Task3_UnitMoveX          260U
#define Patio1_Task3_MoveTimes           18U
#define Patio1_Task3_MoveInterval       400U
uint32_t Patio1_Task3_TotalMove = 0;
#define Patio1_Task3_TotalMoveBound 4800U
#define Patio1_Task3_Interval 250U
#define Patio1_Task3_UnitTurn      ((float)3.2)
#define Patio1_Task3_UnitMove      48
#endif

#if Patio1_Task1
uint32_t Patio1_Task1_CalculateMove(uint8_t state)
{
	if(!state)
		return Patio1_Task1_UnitMoveOff;
	else
		return ((uint32_t)Patio1_Task1_UnitMoveInc)*state;
}
#endif

void StartPatio1Task(void *argument)
{
	uint8_t state = 0;
	int i;
	uint32_t steplength;
	int32_t distance;
	int times;
	UNUSED(state);
	UNUSED(i);
	UNUSED(distance);
	UNUSED(times);
	UNUSED(steplength);
	osThreadSuspend(Patio1TaskHandle);
	PRINTF("Entering Patio 1...\r\n");

	/* Check button */
	Light_Turn(0,(Button1_GPIO_Port->IDR & Button1_Pin) == 0);
	Light_Turn(2,(Button2_GPIO_Port->IDR & Button2_Pin) == 0);
	if(((Button1_GPIO_Port->IDR & Button1_Pin) == 0) && ((Button2_GPIO_Port->IDR & Button2_Pin) == 0))
		goto Patio1_Label_Task4;
	else if((Button2_GPIO_Port->IDR & Button2_Pin) == 0)
		goto Patio1_Label_Task3;
	else if((Button1_GPIO_Port->IDR & Button1_Pin) == 0)
		goto Patio1_Label_Task2;
	else
		goto Patio1_Label_Task1;

Patio1_Label_Task1:
#if Patio1_Task1
	/* Task 1 */
	Patio1_Task1_LineBound = Patio1_Task1_LineBoundInitial;
	OpenMVReset();
	OpenMVCtl(1);
	PRINTF("Patio1 Task1.\r\n");
	OpenMVSet();
	/* Line task */
	for(;;)
	{
		/* Minimum Move */
		osDelay(Patio1_Task1_Interval);
		/* Goto task 2 */
		if(Patio1_Task1_TotalMove>=Patio1_Task1_TotalMoveBound)
		{
			Patio1_Task1_LineBound = Patio1_Task1_UnitMoveBound>>1;
			PRINTF("Critical region.\r\n");
			//Patio1_Task1_TotalMove = 0;
			if(Distance_Get(0)<=Patio1_Task1_DistanceBound)
			{
				PRINTF("Finished task1.\r\n");
				break;
			}
		}
		/* Wait */
		while(!osThreadFlagsWait((uint32_t)1<<OpenMV_Task_Line, osFlagsWaitAll, 0x1FFFF))
			osDelay(1);
		osThreadFlagsClear((uint32_t)1<<OpenMV_Task_Line);
		/* Flip LED */
		Light_Turn(0,2);
		/* Check */
		if(control_s)
		{
			/* Move forward */
			state++;
			if(state >= Patio1_Task1_UnitMoveBound)
				state = Patio1_Task1_UnitMoveBound;
			steplength = Patio1_Task1_CalculateMove(state);
			Move((float)steplength);
			Patio1_Task1_TotalMove += steplength;
		}
		else
		{
			if(state)
			{
				steplength = Patio1_Task1_CalculateMove(state);
				Move(-(float)steplength);
				Patio1_Task1_TotalMove -= steplength;
			}
			state = 0;
			switch(control_t)
			{
			case 0:
				Move((float)Patio1_Task1_UnitMoveOff);
				Patio1_Task1_TotalMove += Patio1_Task1_UnitMoveOff;
				Turn(Patio1_Task1_UnitTurn);
				break;
			case 1:
				Move((float)Patio1_Task1_UnitMoveOff);
				Patio1_Task1_TotalMove += Patio1_Task1_UnitMoveOff;
				Turn(-Patio1_Task1_UnitTurn);
				break;
			default:
				break;
			}
		}
		/* Check distance */
		if((Button2_GPIO_Port->IDR & Button2_Pin) == 0)
		{
			Patio1_Task1_TotalMove += 2000;
			PRINTF("Total Move: %lu.\r\n", Patio1_Task1_TotalMove);
		}
		/* Output distance */
		PRINTF("Distance 0 %lu.\r\n", Distance_Get(0));
	}
	/* Brake */
	if(state)
		MoveBackward(Patio1_Task1_CalculateMove(state));
	/* Stop OpenMV */
	OpenMVCtl(0);
#endif
Patio1_Label_Task2:
#if Patio1_Task2
	/* Task 2 */
	PRINTF("Patio1 Task2.\r\n");
#if 1
	/* Wait to be steady */
	osDelay(5000);
	/* Get distance */
	distance = DistanceAverage(0,12,100);
	distance -= (int32_t)Patio1_Task2_PreferredDistance;
	Move(distance);
	/* Wait to be steady */
	osDelay(5000);
	/* Turn to check direction */
	TurnCW(90);
	/* Wait to be steady */
	osDelay(1500);
	/* Adjust distance */
	MoveBackward(400);
	osDelay(4000);
	/* Precheck */
	while(Distance_Get(2)>3*Patio1_Task2_PreferredDistance)
	{
		Move(50);
		/* Wait to be steady */
		osDelay(400);
	}
	/* Adjust */
	DistanceAdjust();
	//TurnCW(10);
	//DistanceAdjust();
	//TurnCCW(10);
	//DistanceAdjust();
	/* Wait stable */
	osDelay(1000);
	/* Pass bridge */
	Move(100);
	Turn(10);
	osDelay(400);
	Turn(20);
	osDelay(400);
	Move(100);
	Turn(30);
	osDelay(400);
	Move(100);
	Turn(40);
	osDelay(400);
	Move(100);
	Turn(30);
	osDelay(400);
	Turn(30);
	osDelay(400);
	Turn(20);
	osDelay(1000);
#endif
	Patio1_BlockAlong(6);
#if 0
	for(i=0;i<Patio1_Task2_MoveTimes;i++)
	{
		MoveBackward(Patio1_Task2_UnitMoveX);
		osDelay(Patio1_Task2_MoveInterval);
	}
	/* Wait steady */
	osDelay(5000);
	/* Turn back */
	for(i=0;i<4;i++)
	{
		TurnCW(45);
		osDelay(Patio1_Task2_MoveInterval);
	}
#endif
#endif


Patio1_Label_Task3:


#if Patio1_Task3

#if 1
	/* Task 3 */
	PRINTF("Patio1 Task3.\r\n");
	/* Wait to be stable */
	osDelay(3000);
	/* Get distance */
	distance = DistanceAverage(0,12,100);
	distance -= (int32_t)Patio1_Task3_PreferredDistance;
	Move(distance);
	OpenMVDown();
	/* Wait to be steady */
	osDelay(3500);
	/* Turn to check direction */
	TurnCW(90);
	/* Wait to be steady */
	osDelay(3500);
	/* Line again */
	OpenMVReset();
	OpenMVCtl(1);
	//DistanceAdjust();
	osDelay(1000);
	Patio1_Task3_TotalMove = 0;
	OpenMVSet();

	/* Line task */
	for(;;)
	{
		/* Minimum Move */
		osDelay(Patio1_Task3_Interval);
		/* Goto task 2 */
		if(Patio1_Task3_TotalMove>=Patio1_Task3_TotalMoveBound)
		{
			PRINTF("Critical region.\r\n");
			break;
		}
		/* Wait */
		while(!osThreadFlagsWait((uint32_t)1<<OpenMV_Task_Line, osFlagsWaitAll, 0x1FFFF))
			osDelay(1);
		osThreadFlagsClear((uint32_t)1<<OpenMV_Task_Line);
		/* Flip LED */
		Light_Turn(0,2);
		/* Check */
		if(control_s)
		{
			/* Move forward */
			Move((float)Patio1_Task3_UnitMove);
			Patio1_Task3_TotalMove += Patio1_Task3_UnitMove;
		}
		else
		{
			switch(control_t)
			{
			case 0:
				Move((float)Patio1_Task3_UnitMove);
				Patio1_Task3_TotalMove += Patio1_Task3_UnitMove;
				Turn(Patio1_Task3_UnitTurn);
				break;
			case 1:
				Move((float)Patio1_Task3_UnitMove);
				Patio1_Task3_TotalMove += Patio1_Task3_UnitMove;
				Turn(-Patio1_Task3_UnitTurn);
				break;
			default:
				break;
			}
		}
	}
	Move(800);
	osDelay(400);
	Move(800);
#if 0
	/* Precheck */
	while(Distance_Get(2)>2*Patio1_Task3_PreferredDistance)
	{
		Move(50);
		/* Wait to be steady */
		osDelay(400);
	}
	/* Adjust */
	DistanceAdjust();
	osDelay(500);
#endif
#if 0
	/* Pass */
	for(i=0;i<Patio1_Task3_MoveTimes;i++)
	{
		Move(Patio1_Task3_UnitMoveX);
		osDelay(Patio1_Task3_MoveInterval);
	}
#endif
#endif
#endif
Patio1_Label_Task4:

#if 0
	/* Test block */
	Patio1_BlockAlong(142);
#endif
	/* Finished task */
	PRINTF("Finished patio 1.\r\n");
	/* Infinite loop */
	for(;;)
	{
		Light_Turn(0,2);
		osDelay(2000);
	}
	/* Exit */
	osThreadExit();
	return;
}

void Patio1_BlockAlong(uint16_t bound)
{
	int i, j;
	int16_t d;
	/* Init openMV */
	OpenMVReset();
	OpenMVCtl(3);
	OpenMVFront();
	PRINTF("Start block along\r\n");
	OpenMVSet();
	/* Start */
	for(j=0;j<(int)bound;j++)
	{
		/* Wait */
		while(!osThreadFlagsWait((uint32_t)1<<OpenMV_Task_Block, osFlagsWaitAll, 0x1FFFF))
			osDelay(1);
		osThreadFlagsClear((uint32_t)1<<OpenMV_Task_Block);
		if(control_x<320 && control_y<300)
		{
			/* Do */
			d = 160;
			d -= (int16_t)control_x;
			Turn(Pixel2Angle(d));
			osDelay(1800);
			for(i=0;i<4;i++)
			{
				Move(200);
				osDelay(300);
			}
		}
#if 0
		if(control_r>=bound)
			break;
#endif
		osDelay(400);
	}
	/* End */
	PRINTF("End block along\r\n");
	OpenMVReset();
}
